#pragma once

#include "../core/i_alloc.h"
#include "../core/i_blas.h"

#define I_DEFAULT_SCENE_MAX_NR_POINTS     3000000
#define I_DEFAULT_SCENE_MAX_NR_LINES      100000
#define I_DEFAULT_SCENE_MAX_NR_TRIANGLES  100000

namespace idl
{
/*Transform a 3D scene point by
[sR t]
[0  1]*/
template <typename T>
inline void i_transform_3dscene_point(const T x[3], const T R[9], const T t[3], T s, T xp[3])
{
  i_mult_Ax_3x3(R, x, xp);
  i_scale3(xp, s);
  i_add3(t, xp);
}

template <typename T>
class Vis3dSceneBase
{
public:
  Vis3dSceneBase();
  virtual ~Vis3dSceneBase();
  void cleanup();
  bool init(int max_nr_points = I_DEFAULT_SCENE_MAX_NR_POINTS,
            int max_nr_lines = I_DEFAULT_SCENE_MAX_NR_LINES,
            int max_nr_triangles = I_DEFAULT_SCENE_MAX_NR_TRIANGLES);
  
  inline void add_point(T x, T y, T z);
  inline void add_point(const T *X);
  inline void add_point(const T *X, const T R[9], const T t[3], T s);
  inline void add_points(const T *X, int n);
  inline void add_points(const T *const*X, int n);
  inline void add_points(const T *const*X, int n, const T R[9], const T t[3], T s);

  inline void add_line(T x1, T y1, T z1, T x2, T y2, T z2);
  inline void add_line(const T *X1, const T *X2);
  inline void add_line(const T *X1, const T *X2, const T R[9], const T t[3], T s);
  inline void add_lines(const T *const*X, int n);
  inline void add_lines(const T *const*X, int n, const T R[9], const T t[3], T s);

  inline void add_triangle(T x1, T y1, T z1, T x2, T y2, T z2, T x3, T y3, T z3);
  inline void add_triangle(const T *X1, const T *X2, const T *X3);
  inline void add_triangle(const T *X1, const T *X2, const T *X3, const T R[9], const T t[3], T s);
    inline void add_triangles(const T *const *X, int n);
    inline void add_triangles(const T *const *X, int n, const T R[9], const T t[3], T s);

    inline void set_colors(const float *const *color, int n);
    inline void set_colors(const float *color, int n);

    inline void update_points(const T *X, int n);

  const T *const* get_points() const { return _points; }
  const T *const* get_lines() const { return _lines; }
  const T *const* get_triangles() const { return _triangles; }
    const float *const* get_colors() const { return _colors; }

  int get_nr_points() const { return _nr_points; }
  int get_nr_lines() const { return _nr_lines; }
  int get_nr_triangles() const { return _nr_triangles; }
private:
  int _max_nr_points, _max_nr_lines, _max_nr_triangles;
  int _nr_points, _nr_lines, _nr_triangles;
    T **_points, **_lines, **_triangles; 
    float **_colors;
};

    template <typename T> Vis3dSceneBase<T>::Vis3dSceneBase()
    {
        _max_nr_points = 0;
        _max_nr_lines = 0;
        _max_nr_triangles = 0;
        _nr_points = 0;
        _nr_lines = 0;
        _nr_triangles = 0;
        _points = NULL;
        _lines = NULL;
        _triangles = NULL;
        _colors = NULL;
    }

    template <typename T>
     Vis3dSceneBase<T>::~Vis3dSceneBase()
    {
        cleanup();
    }

    template <typename T>
    void Vis3dSceneBase<T>::cleanup()
    {
        i_free2(_points);
        i_free2(_lines);
        i_free2(_triangles); 
        i_free2(_colors);
        _max_nr_points = 0;
        _max_nr_lines = 0;
        _max_nr_triangles = 0;
        _nr_points = 0;
        _nr_lines = 0;
        _nr_triangles = 0;
    }

template <typename T>
bool Vis3dSceneBase<T>::init(int max_nr_points, int max_nr_lines, int max_nr_triangles)
{
  bool ret = true;
    if (max_nr_points > _max_nr_points || max_nr_lines > _max_nr_lines || 
        max_nr_triangles > _max_nr_triangles)
  {
    cleanup();
    _max_nr_points = max_nr_points;
    _max_nr_lines = max_nr_lines;
    _max_nr_triangles = max_nr_triangles;
    _points = i_alloc2<T>(_max_nr_points,        3);
    _lines = i_alloc2<T>(_max_nr_lines*2,        3);
    _triangles = i_alloc2<T>(max_nr_triangles*3, 3);
    _colors = i_alloc2<float>(_max_nr_points, 3);
  }
    if (_points == NULL || _lines == NULL || 
        _triangles == NULL || _colors == NULL)
  {
    cleanup();
    ret = false;
  }
  return ret;
}

template <typename T>
inline void Vis3dSceneBase<T>::add_point(T x, T y, T z)
{
  if (_nr_points<_max_nr_points)
  {
    _points[_nr_points][0] = x;
    _points[_nr_points][1] = y;
    _points[_nr_points][2] = z;
    _nr_points++;
  }
}

template <typename T>
inline void Vis3dSceneBase<T>::add_point(const T *X)
{
  if (_nr_points<_max_nr_points)
  {
    i_copy3(X, _points[_nr_points]);
    _nr_points++;
  }
}

template <typename T>
inline void Vis3dSceneBase<T>::add_point(const T *X, const T R[9], const T t[3], T s)
{
  T xp[3];
  i_transform_3dscene_point(X, R, t, s, xp);
  add_point(xp);
}

template <typename T>
inline void Vis3dSceneBase<T>::add_points(const T *X, int n)
{
  int i, n3 = n*3;
  for (i = 0; i < n3; i+=3){ add_point(X+i); }
}

template <typename T>
inline void Vis3dSceneBase<T>::add_points(const T *const*X, int n)
{
  for (int i = 0; i < n; i++){ add_point(X[i]); }
}

template <typename T>
inline void Vis3dSceneBase<T>::add_points(const T *const*X, int n, const T R[9], const T t[3], T s)
{
  for (int i = 0; i < n; i++){ add_point(X[i], R, t, s); }
}

    template <typename T>
    inline void Vis3dSceneBase<T>::set_colors(const float *const *color, int n)
    {
        int length = i_min(n, _nr_points) * 3;
        i_copy(color[0], _colors[0], length);
    }

    template <typename T>
    inline void Vis3dSceneBase<T>::set_colors(const float *color, int n)
    {
        int length = i_min(n, _nr_points) * 3;
        i_copy(color, _colors[0], length);
    }
    
    template <typename T>
    inline void Vis3dSceneBase<T>::update_points(const T *X, int n)
    {
        _nr_points = 0;
        add_points(X, n);
    }

template <typename T>
inline void Vis3dSceneBase<T>::add_line(T x1, T y1, T z1, T x2, T y2, T z2)
{
  int pos, posp1;
  if (_nr_lines < _max_nr_lines)
  {
    pos = (_nr_lines << 1);
    posp1 = pos + 1;
    _lines[pos][0]   = x1;
    _lines[pos][1]   = y1;
    _lines[pos][2]   = z1;
    _lines[posp1][0] = x2;
    _lines[posp1][1] = y2;
    _lines[posp1][2] = z2;
    _nr_lines++;
  }
}

template <typename T>
inline void Vis3dSceneBase<T>::add_line(const T *X1, const T *X2)
{
  int pos;
  if (_nr_lines < _max_nr_lines)
  {
    pos = (_nr_lines << 1);
    i_copy3(X1, _lines[pos]);
    i_copy3(X2, _lines[pos + 1]);
    _nr_lines++;
  }
}

template <typename T>
inline void Vis3dSceneBase<T>::add_line(const T *X1, const T *X2, const T R[9], const T t[3], T s)
{
  T xp1[3], xp2[3];
  i_transform_3dscene_point(X1, R, t, s, xp1);
  i_transform_3dscene_point(X2, R, t, s, xp2);
  add_line(xp1, xp2);
}

template <typename T>
inline void Vis3dSceneBase<T>::add_lines(const T *const*X, int n)
{
  int i, j;
  for (i = 0, j = 0;  i < n; i++, j+=2){ add_line(X[j], X[j + 1]); }
}

template <typename T>
inline void Vis3dSceneBase<T>::add_lines(const T *const*X, int n, const T R[9], const T t[3], T s)
{
  int i, j;
  for (i = 0, j = 0; i < n; i++, j += 2){ add_line(X[j], X[j + 1], R, t, s); }
}

template <typename T>
inline void Vis3dSceneBase<T>::add_triangle(T x1, T y1, T z1, T x2, T y2, T z2, T x3, T y3, T z3)
{
  int pos, posp1, posp2;
  if (_nr_triangles < _max_nr_triangles)
  {
    pos = 3 * _nr_triangles;
    posp1 = pos + 1;
    posp2 = posp1 + 1;
    _triangles[pos][0] = x1;
    _triangles[pos][1] = y1;
    _triangles[pos][2] = z1;
    _triangles[posp1][0] = x2;
    _triangles[posp1][1] = y2;
    _triangles[posp1][2] = z2;
    _triangles[posp2][0] = x2;
    _triangles[posp2][1] = y2;
    _triangles[posp2][2] = z2;
    _nr_triangles++;
  }
}

template <typename T>
inline void Vis3dSceneBase<T>::add_triangle(const T *X1, const T *X2, const T *X3)
{
  int pos;
  if (_nr_triangles<_max_nr_triangles)
  {
    pos = 3 * _nr_triangles;
    i_copy3(X1, _triangles[pos]);
    i_copy3(X2, _triangles[pos + 1]);
    i_copy3(X3, _triangles[pos + 2]);
    _nr_triangles++;
  }
}

template <typename T>
inline void Vis3dSceneBase<T>::add_triangle(const T *X1, const T *X2, const T *X3, const T R[9], const T t[3], T s)
{
  T xp1[3], xp2[3], xp3[3];
  i_transform_3dscene_point(X1, R, t, s, xp1);
  i_transform_3dscene_point(X2, R, t, s, xp2);
  i_transform_3dscene_point(X2, R, t, s, xp3);
  add_triangle(xp1, xp2, xp3);
}

template <typename T>
inline void Vis3dSceneBase<T>::add_triangles(const T *const*X, int n)
{
  int i, j;
  for (i = 0, j = 0; i < n; i++, j+=3){ add_triangle(X[j], X[j + 1], X[j + 2]) };
}

template <typename T>
inline void Vis3dSceneBase<T>::add_triangles(const T *const*X, int n, const T R[9], const T t[3], T s)
{
  int i, j;
  for (i = 0, j = 0; i < n; i++, j += 3){ add_triangle(X[j], X[j + 1], X[j + 2], R, t, s); }
}


} /*namespace idl*/